
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Int32MultiArray.h"
#include "geometry_msgs/TransformStamped.h"
#include "sensor_msgs/Imu.h"
#include <std_srvs/Empty.h>
#include "tf/tf.h"
#include <chrono>
#include <memory>
#include <iostream>
#include "../../e_wheel/include/data_type_define.hpp"

#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>

float linex, liney, wg;  //下发命令入口
Data_Class *dc;

// wheel vectory is  linear_x,linear_y
double linear_x = 0.0, linear_y = 0.0; //[m/s]
// imu angular vectory
double angular_; // [rad/s]

/// Current pose:
double x_;       //   [m]
double y_;       //   [m]
double heading_; // [rad]
nav_msgs::Odometry odom;

std::vector<double> pose_cov_list_;
std::vector<double> twist_cov_list_;

/// Current timestamp:
ros::Time last_update_timestamp_;

void chatterCallback(const std_msgs::Int32MultiArray::ConstPtr &msg)
{
  // ROS_INFO("I heard: [%s]", msg->data.c_str());
  size_t size;
  float angle_temp;
  int32_t angle_int;
  linear_x =0;
  std::vector<std::int32_t> data_temp;
  data_temp.resize(40, 0);
  size = msg->data.size();
  for (int i = 0; i < size; i++)
    data_temp[i] = msg->data[i];
  dc->data_msg_to_indata(data_temp);
  double spf[4];
  // ROS_INFO("Publishing:speedcur= '%d' '%d' '%d' '%d' '%d' '%d' '%d' '%d'",
  //          dc->speedcur[0],dc->speedcur[1],dc->speedcur[2],dc->speedcur[3],
  //          dc->speedcur[4],dc->speedcur[5],dc->speedcur[6],dc->speedcur[7]
  //          );
  
  angle_int=0;
  for (int i = 0; i < 4; i++)
  {
    spf[i] = dc->convert_rmp_to_ms(dc->get_module_speed_cur(i));
    linear_x += spf[i] / 4.0;
    angle_int+=dc->anglecur[i];
  }
  if(liney!=0)
    {
      angle_temp= (float)(angle_int)*M_PI/4.0/16384.0;

      linear_y = linear_x * sin(angle_temp);
      linear_x = linear_x * cos(angle_temp);
      ROS_INFO("pingcurx=%f  y=%f",linear_x, linear_y);
      ROS_INFO("pingx=%f  y=%f",linex, liney);
    }
    else {linear_y=0.0;}
  // ROS_INFO("I heard speed:ssset= %f =%f %f =%f %f =%f ", linex, linear_x, spf[0], spf[1], spf[2], spf[3]);
  // ROS_INFO("speed:setdd= %d =%d %d =%d  ",dc->get_module_speed_cur(0),dc->get_module_speed_cur(1),dc->speedcur[0],dc->speedcur[1]);
}

void imuCallback(const sensor_msgs::Imu::ConstPtr &msg)
{
  double roll, pitch, yaw;

  //  tf::Quaternion q(msg->orientation.w,msg->orientation.x,msg->orientation.y,msg->orientation.z);
  angular_ = -msg->angular_velocity.z;
  //  tf::Matrix3x3(q).getRPY(roll, pitch, yaw);// 四元数转欧拉角

  // ROS_INFO("v_angle: imu=%f wg=%f", angular_, wg);
}

bool updateOdom(double robot_linear_x, double robot_linear_y, double robot_angular, const ros::Time &time)
{
  std::cout<<"dt:"<<std::endl;
  /// Compute x, y and heading using velocity
  const double dt = (time - last_update_timestamp_).toSec();
  if (dt < 0.0001)
    return false; // Interval too small to integrate with
  
  std::cout<<"odom loop"<<std::endl;

  x_ += ((robot_linear_x * cos(heading_) - robot_linear_y * sin(heading_)) * dt);
  y_ += ((robot_linear_x * sin(heading_) + robot_linear_y * cos(heading_)) * dt);
  heading_ += (angular_ * dt);

  const geometry_msgs::Quaternion orientation(
      tf::createQuaternionMsgFromYaw(heading_));
  odom.header.stamp = time;
  odom.header.frame_id = "odom";
  odom.child_frame_id = "base_link";
  odom.twist.twist.linear.x = linear_x;
  odom.twist.twist.linear.y = linear_y;
  odom.twist.twist.angular.z = angular_;
  odom.pose.pose.position.x = x_;
  odom.pose.pose.position.y = y_;
  odom.pose.pose.position.z = 0.0;
  odom.pose.pose.orientation = orientation;
  odom.pose.covariance = {
      static_cast<double>(pose_cov_list_[0]), 0., 0., 0., 0., 0.,
      0., static_cast<double>(pose_cov_list_[1]), 0., 0., 0., 0.,
      0., 0., static_cast<double>(pose_cov_list_[2]), 0., 0., 0.,
      0., 0., 0., static_cast<double>(pose_cov_list_[3]), 0., 0.,
      0., 0., 0., 0., static_cast<double>(pose_cov_list_[4]), 0.,
      0., 0., 0., 0., 0., static_cast<double>(pose_cov_list_[5])};
  odom.pose.covariance[0] = 0.1;
  odom.pose.covariance[7] = 0.1;
  odom.pose.covariance[35] = 0.2;

  odom.twist.covariance = {
      static_cast<double>(twist_cov_list_[0]), 0., 0., 0., 0., 0.,
      0., static_cast<double>(twist_cov_list_[1]), 0., 0., 0., 0.,
      0., 0., static_cast<double>(twist_cov_list_[2]), 0., 0., 0.,
      0., 0., 0., static_cast<double>(twist_cov_list_[3]), 0., 0.,
      0., 0., 0., 0., static_cast<double>(twist_cov_list_[4]), 0.,
      0., 0., 0., 0., 0., static_cast<double>(twist_cov_list_[5])};
  odom.twist.covariance[0] = 0.1;
  odom.twist.covariance[7] = 0.1;
  odom.twist.covariance[35] = 0.2;
  return true;
}

void resetOdom()
{
  x_ = 0;
  y_ = 0;
  heading_ = 0;

  angular_ = 0;
}

bool reset_odom(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
{
  ROS_INFO("reset odom");
  resetOdom();
  return true;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "yaokong");
  ros::NodeHandle n;
  ros::Publisher control_pub = n.advertise<std_msgs::Int32MultiArray>("e_wheel_ctr", 10);
  ros::Publisher infor_pub = n.advertise<std_msgs::Int32MultiArray>("e_wheel_speed_angle", 10);
  ros::Publisher joy_stick_twist_pub = n.advertise<geometry_msgs::Twist>("joy_stick_twist", 10);
  ros::Publisher odom_twist_pub = n.advertise<geometry_msgs::Twist>("odom_twist", 10);
  ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom",10);

  ros::ServiceServer reset_odom_srv = n.advertiseService("reset_odom",reset_odom);

  ros::Rate loop_rate(50);
  ros::Subscriber sub = n.subscribe("e_wheel_info", 10, chatterCallback);
  ros::Subscriber sub_imu = n.subscribe("/raw_imu", 10, imuCallback);
  // ros::spin();
  dc = new Data_Class();
  auto message = std_msgs::Int32MultiArray();
  message.data.resize(40);
  auto infor = std_msgs::Int32MultiArray();
  infor.data.resize(5);
  for (int i = 0; i < 40; i++)
    message.data[i] = 0;
  size_t count_ = 0;
  int32_t cctt = 10;
  int32_t wheel_out_msg[50], wheel_in_msg[50];

  pose_cov_list_.resize(6);
  pose_cov_list_[0] = 0.001;
  pose_cov_list_[1] = 0.001;
  pose_cov_list_[2] = 1000000;

  pose_cov_list_[3] = 1000000;
  pose_cov_list_[4] = 1000000;
  pose_cov_list_[5] = 1000;

  twist_cov_list_.resize(6);
  twist_cov_list_[0] = 0.001;
  twist_cov_list_[1] = 0.001;
  twist_cov_list_[2] = 1000000;

  twist_cov_list_[3] = 1000000;
  twist_cov_list_[4] = 1000000;
  twist_cov_list_[5] = 1000;

  while (ros::ok())
  {
    std_msgs::String msg;
    std::stringstream ss;
    // ss << "hello world " << count_;
    // msg.data = ss.str();
    // ROS_INFO("%s", msg.data.c_str());
    // float linex,wg;
    linex = dc->convert_yaogan_line(dc->yaogan[1]);
    wg = dc->convert_yaogan_wmga(dc->yaogan[2]);
    liney = dc->convert_yaogan_line(-dc->yaogan[0])*0.6;///横向平移最大0.6米/秒
    if(wg!=0)liney=0;   //平移和转弯不能并存
    dc->yaokong_real(linex, wg,liney);

    dc->data_control_to_msg(message.data);

    //  ROS_INFO("this run %d", dc->yaogan[0]);
    // dc->yaokong();
    // dc->data_control_to_msg(message.data);
    infor.data[0] = (int32_t)(linex * 100.0);
    infor.data[1] = (int32_t)(linear_x * 100.0);

    infor.data[2] = (int32_t)(wg * 1000.0);
    infor.data[3] = (int32_t)(angular_ * 1000);

    geometry_msgs::Twist joy_stick_twist;
    joy_stick_twist.linear.x = linex;
    joy_stick_twist.angular.z = wg;
    joy_stick_twist_pub.publish(joy_stick_twist);

    geometry_msgs::Twist odom_twist;
    odom_twist.linear.x = linear_x;
    odom_twist.angular.z = angular_;
    odom_twist_pub.publish(odom_twist);
    
    
    ros::Time now_ = ros::Time::now();
    // calculate odom
    updateOdom(linear_x,linear_y,angular_,now_);
    odom_pub.publish(odom);
    last_update_timestamp_ = now_;

    infor.data[4] = 1133;
    control_pub.publish(message);
    infor_pub.publish(infor);
    ros::spinOnce();
    // ros::spin();
    loop_rate.sleep();
  }
  return 0;
}